clc
clear all
close all 
% 状态量 x = [x y z vx vy vz q0 q1 q2 q3 omega_x omega_y omega_z]
global Init_Data State_Watch r_t
% Init_Data 质量 m,
% 惯性半径r_Gx,r_Gy,r_Gz,
% 气动参数 Cx0,Cy0,Cy_a,Cz0,Cz_b,
% 参考面积 S_ref,重力加速度 g_0,空气密度 rho
% 弹体气动力到质心距离 r_TBx,r_TBy,r_TBz,
% 舵面气动力到质心距离 r_TDx,r_TDy,r_TDz
num = 1;
Init_Data(num) = 0.3;num=num+1;
Init_Data(num) = 0.01;num=num+1;Init_Data(num) = 0.05;num=num+1;Init_Data(num) = 0.05;num=num+1;
Init_Data(num) = 0.01;num=num+1;Init_Data(num) = 0.00;num=num+1;Init_Data(num) = 0.25;num=num+1;Init_Data(num) = 0.00;num=num+1;Init_Data(num) = 0.25;num=num+1;
Init_Data(num) = 0.025^2;num=num+1;Init_Data(num) = 9.81;num=num+1;Init_Data(num) = 1.4;num=num+1;
Init_Data(num) = -0.05;num=num+1;Init_Data(num) = 0;num=num+1;Init_Data(num) = 0;num=num+1;
Init_Data(num) = 0.1;num=num+1;Init_Data(num) = 0;num=num+1;Init_Data(num) = 0;num=num+1;
%[t,y]=ode45(@ode_dt,[0 X(1,end)],X(2:18,1));
%r_t =[22.4175;0.4965;0.3*(rand-0.5)];
r_t =[22.4175;0.4965;0.2];
%无干扰
x0 = [0,0,0,15*cosd(45),15*sind(45),0,my_angle2quat([deg2rad(45);deg2rad(0);0])',0,0,0];
% 加随机干扰
%x0 = [0,0,0,15*cosd(45),15*sind(45),0,my_angle2quat([deg2rad(45+6*(rand-0.5));deg2rad(6*(rand-0.5));0])',0,0,0];
% 标称弹道 2.12s 22.4175 0.4965
%x0 = [0,0,0,15*cosd(45),15*sind(45),0,my_angle2quat([deg2rad(45);deg2rad(0);0])',0,0,0];
stop_cond = @(t, y) y(2,1) < 0.5 && t>1;
[t,y]=rk4_fixed_step_stop(@ode_dt,[0 100],x0,0.005,stop_cond);
%[t,y]=ode45(@ode_dt,[0 1],x0,0.01);
y_ = y.';
t_ = t.';
fprintf('落点偏差:%.2f,%.2f,%.2f\n',y_(1,end)-r_t(1),y_(2,end)-r_t(2),y_(3,end)-r_t(3));
disp("计算完成")


%% 
% figure(1);
% plot(t_,y_(2,:))
% hold on 
% plot(tp,state(2,:));
% legend("x_z","x_p");
plot_num = 1;
angle_t = my_quat2angle(y_(7:10,:));

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(2,:), State_Watch(3,:));
title('纵向平面');

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(2,:), State_Watch(4,:));
title('侧向平面');



% 绘制三维轨迹 x y z y向上 xp yp zp z向上 x--xp y---z z--y
figure(plot_num);plot_num = plot_num + 1;
plot3(State_Watch(2,:), State_Watch(4,:), State_Watch(3,:), 'b', 'LineWidth', 2); 
hold on;
scatter3(State_Watch(2,1), State_Watch(4,1), State_Watch(3,1), 100, 'g', 'filled'); % 标记起点
scatter3(State_Watch(2,end), State_Watch(4,end), State_Watch(3,end), 100, 'r', 'filled'); % 标记终点
hold off;
% 设置坐标轴
xlabel('North (X)');
ylabel('East (Z)');
zlabel('Up (Y)');
title('3D 轨迹 (左手坐标系，Y 轴朝上)');
set(gca, 'ZDir', 'normal', 'YDir', 'reverse');
%axis equal;
%daspect([1 1 1])
%view(45, 30); % 视角调整
%view(0, 90); % 使 Y 轴朝上

figure(plot_num);plot_num = plot_num + 1;
plot(t_,angle_t(1,:),t_,angle_t(2,:),t_,angle_t(3,:)) 
legend("phi","psi","gamma");
title('2-3-1欧拉角(deg)')

% figure(plot_num);plot_num = plot_num + 1;
% plot(t_,angle_t(1,:)-State_Watch(15,:),t_,angle_t(2,:)-State_Watch(16,:),t_,angle_t(2,:)-State_Watch(17,:)) 
% legend("phi","psi","gamma");
% title('angle error')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),State_Watch(18,:),State_Watch(1,:),State_Watch(19,:)) 
legend("alpha","beta");
title('攻角和侧滑角随时间变化(deg)')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),State_Watch(20,:),State_Watch(1,:),State_Watch(21,:),State_Watch(1,:),State_Watch(22,:)) 
legend("vbx","vby","vbz");
title('弹体系速度(m/s)')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),rad2deg(State_Watch(23,:)),State_Watch(1,:),rad2deg(State_Watch(24,:)),State_Watch(1,:),rad2deg(State_Watch(25,:))) 
legend("omega_x","omega_y","omega_z");
title('弹体角速度(deg)')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),State_Watch(26,:),State_Watch(1,:),State_Watch(27,:),State_Watch(1,:),State_Watch(28,:)) 
legend("Fbx","Fby","Fbz");
title('弹体系中弹体气动力(N)')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),State_Watch(29,:),State_Watch(1,:),State_Watch(30,:),State_Watch(1,:),State_Watch(31,:)) 
legend("awx","awy","awz");
title('视加速度(m/s^2)')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),State_Watch(32,:),State_Watch(1,:),State_Watch(33,:)) 
legend("delta_z","delta_y");
title('舵偏(度)')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),State_Watch(32,:)-State_Watch(18,:),State_Watch(1,:),State_Watch(33,:)+State_Watch(19,:),...
    State_Watch(1,:),30*ones(size(State_Watch(1,:))),State_Watch(1,:),-30*ones(size(State_Watch(1,:)))) 
hold on 
legend("delta_z","delta_y","delta_zmax","delta_ymax");
title('失速实际舵偏(度)')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),State_Watch(34,:)) 
legend("q_{norm}");
title('四元数归一化')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),State_Watch(35,:),State_Watch(1,:),State_Watch(36,:),State_Watch(1,:),State_Watch(37,:)) 
legend("Fdx","Fdy","Fdz");
title('弹体系中舵面气动力(N)')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),State_Watch(38,:),State_Watch(1,:),State_Watch(39,:)) 
legend("ny","nz");
title('比例导引过载指令(G)')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),State_Watch(38,:),State_Watch(1,:),State_Watch(39,:),State_Watch(1,:),State_Watch(30,:)/9.81,State_Watch(1,:),State_Watch(31,:)/9.81) 
legend("ny","nz","awy","awz");
title('比例导引过载指令对比(G)')


figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),rad2deg(State_Watch(40,:)),State_Watch(1,:),rad2deg(State_Watch(41,:)),State_Watch(1,:),rad2deg(State_Watch(42,:))) 
legend("omegax_t","omegay_t","omegaz_t");
title('视线角速度(度)')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),(State_Watch(43,:)),State_Watch(1,:),(State_Watch(44,:))) 
legend("control1I","control2I");
title('误差积分项')

figure(plot_num);plot_num = plot_num + 1;
plot(State_Watch(1,:),(State_Watch(45,:)),State_Watch(1,:),(State_Watch(46,:)),State_Watch(1,:),(State_Watch(47,:))) 
legend("x_ref","y_ref","z_ref");
title('相对位置')


% figure(plot_num);plot_num = plot_num + 1;
% plot(State_Watch(1,:),(State_Watch(45,:)),State_Watch(1,:),(State_Watch(46,:))) 
% legend("Ny","Nz");
% title('比例导引计算过载(G)')